package com.gitee.wsl.mathematics.coordinate.cylindrical

import com.gitee.wsl.mathematics.vector.ext.atan2
import com.gitee.wsl.mathematics.vector.vec3.Vec3
import com.gitee.wsl.mathematics.vector.vec3.Vec3f
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.ext.cos
import com.gitee.wsl.unit.angle.ext.sin
import kotlin.math.sqrt


data class CylindricalVector(
    val radius: Double = 1.0,
    val angle: AngleUnit = AngleUnit.ZERO,
    val y: Double = 0.0,
) {
    fun toVector3() = toCartesian(this)

    companion object {
        fun fromCartesian(v: Vec3f): CylindricalVector = fromCartesian(v.x, v.y, v.z)
        fun fromCartesian(v: Vec3): CylindricalVector = fromCartesian(v.x, v.y, v.z)
        inline fun fromCartesian(x: Number, y: Number, z: Number): CylindricalVector = fromCartesian(x.toDouble(), y.toDouble(), z.toDouble())
        fun fromCartesian(x: Double, y: Double, z: Double): CylindricalVector = CylindricalVector(
            radius = sqrt(x * x + z * z),
            angle = AngleUnit.atan2(x, z),
            y = y,
        )

        fun toCartesian(c: CylindricalVector): Vec3 = toCartesian(c.radius, c.angle, c.y)
        fun toCartesian(radius: Double, angle: AngleUnit, y: Double): Vec3 = Vec3(
            x = radius * angle.sin(),
            y = y,
            z = radius * angle.cos(),
        )
    }
}

fun Vec3.toCylindrical(): CylindricalVector = CylindricalVector.fromCartesian(this)
